#include "Basic Structure.h"



void Pose::toEulerAngle(double *p)const
{
    auto q = toQuaternion();
    double q0 = q.x();
    double q1 = q.y();
    double q2 = q.z();
    double q3 = q.w();
    
    p[0] =       atan2(2 * (q0*q1 + q2*q3), 1 - 2 * (q1*q1 + q2*q2));
    p[1] =       asin( 2 * (q0*q2 - q3*q1));
    p[2] =       atan2(2 * (q0*q3 + q1*q2), 1 - 2 * (q2*q2 + q3*q3));
}



Eigen::Vector4d Pose::toQuaternion()const
{
    Eigen::Vector4d ret;
    ceres::RotationMatrixToQuaternion(R.transpose().data(), ret.data());
    return ret;
}

Eigen::Vector3d Pose::EulerAngle()const
{
    return R.eulerAngles(0, 1, 2);
}



Eigen::Vector3d Pose::map(const Eigen::Vector3d&a,Eigen::Matrix<double,3,6>&J)const
{
    J.setZero();
    J(0,4) = a.z();
    J(1,3) = -a.z();
    J(0,5) = -a.y();
    J(2,3) = a.y();
    J(1,5) = a.x();
    J(2,4) = -a.x();
    J(0,0) = 1;
    J(1,1) = 1;
    J(2,2) = 1;
    return R*a+t;
}

Eigen::Vector2d IntrinsicMatrix::project(const Eigen::Vector3d& a,Eigen::Matrix<double,2,3>&J)const
{
    J.setZero();
    double x = a.x();
    double y = a.y();
    double invz = 1/a.z();
    double invz2 = invz*invz;
    Eigen::Vector2d b;
    b(0) = x*invz*fx+cx;
    b(1) = y*invz*fx+cy;
    J(0,0) = J(1,1)= fx*invz;
    J(0,2) = -x*invz2*fx;
    J(1,2) = -y*invz2*fx;
    return b;
}


SingleScaleStructrue::SingleScaleStructrue(const SingleScaleStructrue& a)
{
  this->desp = a.desp.clone();
  this->img = a.img.clone();
  this->kpts = a.kpts;
  this->p3d = a.p3d;
}




MultiScaleStructure::MultiScaleStructure(const MultiScaleStructure&a)
{
  this->allKpts = a.allKpts;
  this->allP3Ds = a.allP3Ds;
  allImgs.resize(a.allImgs.size());
  allDesp.resize(a.allDesp.size());
  for (int i=0; i<allImgs.size(); i++) {
    allImgs[i] = a.allImgs[i].clone();
    allDesp[i] = a.allDesp[i].clone();
  }
  scale = a.scale;
  levelNum = a.levelNum;
    this->allFts = a.allFts;
}
//void Pose::applyTranform(const double * p)
//{
//	Matx33d Rx = Matx33d::zeros();
//	auto d = Rx.val;
//	d[1] = -p[2];
//	d[3] = p[2];
//	d[2] = p[1];
//	d[6] = -p[1];
//	d[5] = -p[0];
//	d[7] = p[0];
//	R = R - Rx*R;
//	t(0) += p[3];
//	t(1) += p[4];
//	t(2) += p[5];
//}

IntrinsicMatrix::IntrinsicMatrix(const IntrinsicMatrix & b)
{
	K = b.K;
	fx = b.fx;
	cx = b.cx;
	fy = b.fy;
	cy = b.cy;
}

IntrinsicMatrix IntrinsicMatrix::applyScale(double scale)const
{
    Eigen::Matrix3d k = K*scale;
	k(2, 2) = 1;
	return IntrinsicMatrix(k);
}

Eigen::Vector3d IntrinsicMatrix::cam2world(const Eigen::Vector2d&a)const
{
    Eigen::Vector3d ret;
    ret(0) = a(0) - cx;
    ret(1) = a(1) - cy;
    ret(2) = fx;
    ret.normalize();
    return ret;
}


Eigen::Vector2d IntrinsicMatrix::project(const Eigen::Vector3d& a)const
{
    Eigen::Vector2d b;
    b(0) = a.x()/a.z()*fx+cx;
    b(1) = a.y()/a.z()*fx+cy;
    return b;
}

MultiScaleStructure::MultiScaleStructure()
{
    
}

MultiScaleStructure::MultiScaleStructure(const Mat &img, float scale, int levelNum)
{
    setPyramid(img, levelNum, scale, allImgs);
    allKpts.resize(levelNum);
    allDesp.resize(levelNum);
    allP3Ds.resize(levelNum);
    allFts.resize(levelNum);
    this->scale = scale;
    this->levelNum = levelNum;
}